//#include <NewSoftSerial.h>

#include <Spi.h>
//#include <SoftwareSerial.h>
#include <Servo.h>
#include <Firmata.h>
#include <Wire.h>
//#include <math.h>

#define STATUS_LED 13
#define SEC 1000000
#define HZ 100

long timer=0;   //general purpuse timer
long timer_old;

int status = LOW;
int counter = 0;
int n=0;

#define ADC_CHANNELS 16
#define ADC_MAXCOUNT 200

volatile uint8_t MuxSel=0;
volatile uint8_t analog_reference;
volatile uint32_t analog_buffer[ADC_CHANNELS];
volatile uint16_t analog_count[ADC_CHANNELS];

//NewSoftSerial serial1(0,1);

//ADC VARIABLES
int AN[ADC_CHANNELS] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; //array that store the 3 ADC filtered data (gyros)
int AN_OFFSET[ADC_CHANNELS] = {0,0,0,0,0,0,0,0,0,0,0,0,0,0,0,0}; //Array that stores the Offset of the sensors

//INPUTSERIAL VARIABLES
#define MAXBUFF 128
uint8_t s1buff[MAXBUFF];
int s1blen = 0;
uint8_t s2buff[MAXBUFF];
int s2blen = 0;
int s2bsta = 0;
uint8_t s3buff[MAXBUFF];
int s3blen = 0;

uint8_t s1ftmpbuf[MAXBUFF];
int s1ftmplen = 0;
int s1statecount = 0;
uint8_t s1statebuf[4];
boolean s1parsing = false;
uint8_t s1mode = 0; //No frame
int s1footercount = 0;
uint8_t* s1foutbuf;
int s1foutlen = 0;

uint8_t s2ftmpbuf[MAXBUFF];
int s2ftmplen = 0;
int s2statecount = 0;
uint8_t s2statebuf[4];
boolean s2parsing = false;
uint8_t s2mode = 0; //No frame
int s2footercount = 0;
uint8_t* s2foutbuf;
int s2foutlen = 0;

boolean s1frame = false;
boolean s2frame = false;
boolean s3frame = false;

boolean s1start = false;
boolean s2start = false;
boolean s3start = false;

//POLOLU SERVOS
#define MAXSERVO 8
#define MINPULSE 2000
#define MAXPULSE 4000
#define ZEROPULSE 3000

#define MAXJOYVAL 1024 //0-1024
#define MAXVAL 500 //-500->+500


int degpos[MAXSERVO] = {0,0,0,0,0,0,0,0};
int servopos = MINPULSE;
//Vettore per il joystick
double vettore[2] = {0,0}; //0-360 (angolo), 0-45 (modulo)

Servo motor;
int jx = 0;
int jy = 0;
int jz = 0;
int jm = 0;

#define MAXPKSVAL 4096 //0-4096
int pksv[MAXSERVO+1] = {0,0,0,0,0,0,0,0,0};

//#define FAILSAFEMAX 50
//int failsafe = 0;
#define FAILSAFEMAXMILLIS 500
long tmrfs = 0;
bool flgFailsafe = false;

#define SENDCMDMINTIME 10
bool flgSendCommand = false;
long tmrlastcmd = 0;

#define MOTORSAT 3200
#define MOTORGAIN 0.7

//#define JOYRADIO 1

void setup()
{ 
  motor.attach(8);
  motor.writeMicroseconds(MINPULSE / 2);
  //serial1.begin(57600);           //Setting baud rate for serial communication
  //Serial.begin(115200);
  //Serial.beginEx(19300,true);
  //Serial.beginEx(57800,true); //57600  -debug
  //Serial.begin(57600);
  //Serial1.begin(57600);
//#ifdef JOYRADIO == 1
//  Serial2.begin(14400); //38400        -xbee
//#else
  //Serial2.begin(9600); //38400        -xbee
  //Serial2.begin(38400); //57600        -xbee
  Serial2.begin(38800); //57600        -xbee
  //Serial2.begin(19200); //57600        -xbee
//#endif
  Serial3.begin(50000); //38400 no u2x -pololu
  
  pinMode (STATUS_LED,OUTPUT);  // Status LED
  
  Analog_Reference(DEFAULT);     //Setting 5V of voltage reference for the ADC conversion
  Analog_Init();                 //Setting ADC
  I2C_Init();

  //Serial.println("UNIBO Concentratore!");

  digitalWrite(STATUS_LED,status);
  delay(500);

  // Initialze ADC readings and buffers
  Read_adc_raw();
  delay(20);

  for(int i=0;i<32;i++)    // We take some readings...
    {
    Read_adc_raw();
    for(int y=0; y<ADC_CHANNELS; y++)   // Cumulate values
      AN_OFFSET[y] += AN[y];
    delay(20);
    }
    
  for(int y=0; y<ADC_CHANNELS; y++)
    AN_OFFSET[y] = AN_OFFSET[y]/32;

  delay(1000);
  digitalWrite(STATUS_LED,HIGH);
  Read_adc_raw();     // ADC initialization
  timer=micros();
  delay(1);
  tmrlastcmd = millis();
}


int demolt = -1;
//#define MAX 2
//Una seriale viagga a MainLoop (100Hz), le altre alla metà
#define PRIORITYSERIAL 1

int tcount = 0;
#define FRMLEN 20


int i = 0;
uint16_t tstamp = 0;
uint16_t fcount = 0;

void loop() //Main Loop
{
    
    /*
    Serial.print("##");
    Serial.print(millis());
    Serial.print("##");
    Serial.println();    
    */

    //TEST FRAME
    /*
    while (Serial2.available()>0){
      uint8_t incbyte = Serial2.read();
      Serial.print(incbyte, HEX);
      Serial.print(" ");
      tcount++;
      if (tcount >= FRMLEN){
        Serial.println();
        tcount = 0;
      }
    }
    */
    
    //Se ho pacchetto intero, rispedisco!
    //TODO!
    /*
    if (s1frame){
      Serial.write(s1foutbuf,s1foutlen);
      //Serial.println("OUT!");
      Serial.println("||");
      Serial.print(s1foutlen,DEC);
      Serial.println("||");
    }
    */

    
    readSerial2();
      
    if (s2frame){
      //Serial.println("PPPPPPPPPPPPPPPPPPPPPPPPPP");
      s2frame = false;
      //Check length
      int len = s2foutbuf[4];
      len = len <<8;
      len += s2foutbuf[5];
      //Serial.println();
      //Serial.print(len,HEX);
      //Serial.print(" - ");
      //Serial.print(s2foutlen,HEX);
      //Serial.print(" - ");
      if (len == s2foutlen){
        //Serial.println("OK");
        //Sono ragionevolmente arrivati tutti i bytes...
        //Joystick parser
        pksv[0] = Parse2Bytes(s2foutbuf,8);
        pksv[1] = Parse2Bytes(s2foutbuf,10);
        pksv[2] = Parse2Bytes(s2foutbuf,12);
        pksv[3] = Parse2Bytes(s2foutbuf,14);
        pksv[4] = Parse2Bytes(s2foutbuf,16);
        pksv[5] = Parse2Bytes(s2foutbuf,18);
        pksv[6] = Parse2Bytes(s2foutbuf,20);
        pksv[7] = Parse2Bytes(s2foutbuf,22);
        pksv[8] = Parse2Bytes(s2foutbuf,24);
        tstamp = Parse2Bytes(s2foutbuf,26);
        fcount++;
        if (fcount > 1000){
          fcount = 0;
        }
//#ifdef JOYRADIO == 1
//          double js = MAXJOYVAL / 2000.0;
//          jx = (jx-2000) * js ;
//          jy = (jy-2000) * js;
//          jz = (jz-2000) * js;
//          jm = (jm-2000) * js;
//#else
//#endif
        //Serial.print(fcount);
        //Serial.print(" - ");
        for (i=0;i<(MAXSERVO+1);i++){
          pksv[i] = pksv[i]-(MAXPKSVAL/2);
          pksv[i] = pksv[i]/2;
          pksv[i]+= ZEROPULSE;

          //Serial.print(pksv[i]);
          //Serial.print(" - ");

        }
        //Serial.print(tstamp);
        //Serial.println();
        tmrfs = millis();
        digitalWrite(STATUS_LED,HIGH);     
        flgSendCommand = true;
        //Serial.println("OK");
      }else{
        //Serial.println("--KO");
        digitalWrite(STATUS_LED,LOW);     

      }
    }
    //failsafe++;
    if (tmrfs == 0){
        for (i=0;i<(MAXSERVO+1);i++){
          pksv[i] = pksv[i]-(MAXPKSVAL/2);
          pksv[i] = pksv[i]/2;
          pksv[i]+= ZEROPULSE;
        }
        pksv[8] = MINPULSE;
    }else{
      if (flgFailsafe || (millis()-tmrfs >= FAILSAFEMAXMILLIS) ){
        for (i=0;i<(MAXSERVO+1);i++){
          pksv[i] = pksv[i]-(MAXPKSVAL/2);
          pksv[i] = pksv[i]/2;
          pksv[i]+= ZEROPULSE;
        }
        pksv[8] = MINPULSE;
        flgFailsafe = true;
      }
    }
    
    if (millis()-tmrlastcmd >= SENDCMDMINTIME){
      flgSendCommand = true;
    }
    
    if (flgSendCommand){
      //Gestire MOTORGAIN
//      int motorscale = (ZEROPULSE-MINPULSE)/MAXPKSVAL;
//      int motormsec = (ZEROPULSE+pksv[MAXSERVO]*motorscale);
      int motormsec = pksv[MAXSERVO];
      if (motormsec > MOTORSAT){
        motormsec = MOTORSAT;
      }
      motor.writeMicroseconds(motormsec / 2);
      //Test pololu
      //servopos = MAXPULSE;
      //CalcolaVettore(jx, jy, vettore);
      //Serial.println(vettore[0]);
      //Serial.println(vettore[1]);
      //for (int i = 0; i< MAXSERVO; i++){
      //  //degpos[i] = servopos;
      //  degpos[i] = ZEROPULSE;
      //}
      
      //CalcolaServi(vettore, degpos);
      //CalcolaRotazione(jz, degpos);
      for (i=0;i<MAXSERVO;i++){
        degpos[i] = pksv[i];
      }
      SetServos(degpos);
      
      flgSendCommand = false;
      tmrlastcmd = millis();
    }
    
    //Serial.println(millis());
}


